#include "CAN.h"
#include "IO_DEF.h"

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

/*******************TWAI*******************/

twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)TWAI_TX, (gpio_num_t)TWAI_RX, TWAI_MODE_NORMAL);
twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();  //Look in the api-reference for other speed sets.
twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

uint8_t TWAI_CAN_Init(CAN_SPEED_t speed)
{
  switch(speed)
  {
    case CAN_1000K:
      t_config.brp = 4;
    break;
    case CAN_500K:
      t_config.brp = 8;
    break;
    case CAN_250K:
      t_config.brp = 16;
    break;
    case CAN_125K:
      t_config.brp = 32;
    break;
  }
  // Install TWAI driver
  if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) 
  {
    return 0;
  }
  // Start TWAI driver
  if (twai_start() != ESP_OK) 
  {
    return 0;
  }
  // Reconfigure alerts to detect frame receive, Bus-Off error and RX queue full states
  uint32_t alerts_to_enable = TWAI_ALERT_RX_DATA | TWAI_ALERT_ERR_PASS | TWAI_ALERT_BUS_ERROR | TWAI_ALERT_RX_QUEUE_FULL;
  if (twai_reconfigure_alerts(alerts_to_enable, NULL) != ESP_OK) 
  {
    return 0;
  }
  return 1;
}

uint8_t TWAI_ERROR=0;
twai_status_info_t status_info;


void TWAI_CAN_RECOVERY_TASK(void *arg)
{
  gpio_set_direction(LED_OFF,GPIO_MODE_DEF_OUTPUT);
	while(1)
	{
		twai_get_status_info(&status_info);
		if(status_info.state==TWAI_STATE_BUS_OFF)
		{
			TWAI_ERROR=1;
			gpio_set_level(LED_OFF,1);
			twai_initiate_recovery();
		}
		else if(status_info.state==TWAI_STATE_STOPPED)
		{
			twai_start();
		}
		else if(status_info.state==TWAI_STATE_RUNNING)
		{
			TWAI_ERROR=0;
			gpio_set_level(LED_OFF,0);
		}
		vTaskDelay(1000);
	}
}

uint8_t TWAI_CAN_STATUS(void)
{
	return !TWAI_ERROR;
}

esp_err_t TWAI_CAN_TX(twai_message_t *msg)
{
	if(TWAI_ERROR) return ESP_FAIL;
	return twai_transmit(msg,100);
}

esp_err_t TWAI_CAN_RX(twai_message_t *msg)
{
	return twai_receive(msg,5);
}
